/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.HiTechnicColorSensor;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;
import edu.wpi.first.wpilibj.templates.commands.auton;
import edu.wpi.first.wpilibj.templates.commands.autonomous2;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends IterativeRobot {
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
//    public Gyro gyro=new Gyro(1);
//    public AnalogChannel sonar = new AnalogChannel(2);
//    public AxisCamera camera;
//    public HiTechnicColorSensor colorSensor = new HiTechnicColorSensor (1);
//    public LiveWindow liveWindow= new LiveWindow();
//    public ADXL345_I2C accel = new ADXL345_I2C(1, ADXL345_I2C.DataFormat_Range.k2G);
    

    public void robotInit() {
        // instantiate the command used for the autonomous period
//        autonomousCommand = new ExampleCommand();
//        gyro.reset();
        CommandBase.init();
   
//        camera.writeResolution(AxisCamera.ResolutionT.k320x240);
//        camera.writeMaxFPS(18);
        RobotMap.endGameServo.setAngle(170);
       
        // Initialize all subsystems
    }
    
    public void disabledInit(){
        
    }

    public void autonomousInit() {
        // schedule the autonomous command (example)
        auton auto = new auton();
        auto.start();
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
        
    }

    public void teleopInit() {
	// This makes sure that the autonomous stops running when
        // teleop starts running. If you want the autonomous to 
        // continue until interrupted by another command, remove
        // this line or comment it out.
        
//        autonomousCommand.cancel();
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
//        liveWindow.addSensor("RobotTemplate","Gyro",Gyro);
        
        Scheduler.getInstance().run();
        System.out.println("Upper"+RobotMap.upperLimit.get());
//        System.out.println("Lower"+RobotMap.lowerLimit.get());
//        System.out.println("Middle"+RobotMap.middleLimit.get());
//        System.out.println("Slow"+RobotMap.slowLimit.get());
//        double angle=gyro.getAngle();
//        double distance=sonar.getVoltage()*100;
//        int redValue = colorSensor.getRed();
//        int blueValue = colorSensor.getBlue();
//        int greenValue = colorSensor.getGreen();

//        ADXL345_I2C.AllAxes axis1 = accel.getAccelerations();
         
//        SmartDashboard.putNumber("Red:", redValue);
//        SmartDashboard.putNumber("Blue:", blueValue);
//        SmartDashboard.putNumber("Green:",greenValue);
//        SmartDashboard.putNumber("X-Axis:",axis1.XAxis);
//        SmartDashboard.putNumber("Y-Axis:",axis1.YAxis);
//        SmartDashboard.putNumber("Z-Axis:",axis1.ZAxis);

//        
//        SmartDashboard.putString("Frisbee Color", output);
//        SmartDashboard.putNumber("Angle", angle);
//        SmartDashboard.putNumber("Image", image);
//        SmartDashboard.putNumber("Distance", distance);
        

    }
    
    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
        LiveWindow.run();
    }
}
